#region Using Statements
using System;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Content;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;
using System.Collections.Generic;
using SkinnedModel;

#endregion

namespace SkinnedModel{

    public interface EffectorMotion
    {
        //updates td
        //returns interpolated value
        Vector3 UpdatePosition(float dt);

        //Has this motion completed?
        bool IsFinished();
        string GetRoot();

        void ExtractIKGoal(ref IKSolver ikSolver, TimeSpan elapsedTime);
    }

    /// <summary>
    /// Class for defining linear end effector motion
    /// </summary>
    public class LinearEffectorMotion : EffectorMotion
    {
        protected Vector3 start;
        protected Vector3 end;
        protected float span, t;
        
        protected string root;
        protected string effector;

        public LinearEffectorMotion(Vector3 start, Vector3 end, float span,  string root, string effector)
        {
            this.start = start;
            this.end = end;
            this.span = span;
            t = 0;
            this.root = root;
            this.effector = effector;
        }

        //updates td
        //returns interpolated value
        public Vector3 UpdatePosition(float dt)
        {
            t += dt;
            t = TMath.Clamp(t, 0.0f, span);
            float nt = t / span;

            return (1.0f - nt) * start + nt * end;
        }

        //Has this motion completed?
        public bool IsFinished()
        {
            return t >= span;
        }

        public string GetRoot()
        {
            return root;
        }

        public void ExtractIKGoal(ref IKSolver ikSolver, TimeSpan elapsedTime)
        {
            Vector3 target = UpdatePosition((float)elapsedTime.TotalSeconds);
            ikSolver.AddReachGoal(effector, root, target);
        }

    }

    public class LinearAimMotion : LinearEffectorMotion
    {


        public LinearAimMotion(Vector3 start, Vector3 end, float span,  string root, string effector):
            base(start, end, span, root, effector)
        {
        }

        new public void ExtractIKGoal(ref IKSolver ikSolver, TimeSpan elapsedTime)
        {
            Vector3 aim = UpdatePosition((float)elapsedTime.TotalSeconds);
            ikSolver.AddOneJointAimGoal(effector, aim);
        }
    }

}